﻿package bsplib;
import flash.geom.Matrix;

class Matrix4
{
    public var m11 : Float;
    public var m12 : Float;
    public var m13 : Float;
    public var m14 : Float;
    public var m21 : Float;
    public var m22 : Float;
    public var m23 : Float;
    public var m24 : Float;
    public var m31 : Float;
    public var m32 : Float;
    public var m33 : Float;
    public var m34 : Float;
    public var m41 : Float;
    public var m42 : Float;
    public var m43 : Float;
    public var m44 : Float;

    public function new() {}
    
    public inline function clone() : Matrix4 {
        return new Matrix4().copy(this);
    }
    
    public inline function copy( v : Matrix4 ) : Matrix4 {
        m11 = v.m11; m12 = v.m12; m13 = v.m13; m14 = v.m14;
        m21 = v.m21; m22 = v.m22; m23 = v.m23; m24 = v.m24;
        m31 = v.m31; m32 = v.m32; m33 = v.m33; m34 = v.m34;
        m41 = v.m41; m42 = v.m42; m43 = v.m43; m44 = v.m44;
        return this;
    }
    
    public inline function cIdentityMat () : Matrix4 {
        m11 = 1; m12 = 0; m13 = 0; m14 = 0;
        m21 = 0; m22 = 1; m23 = 0; m24 = 0;
        m31 = 0; m32 = 0; m33 = 1; m34 = 0;
        m41 = 0; m42 = 0; m43 = 0; m44 = 1;
        return this;
    }
    
    public inline function cTranslationMat( x : Float, y : Float, z : Float ) : Matrix4 {
        m11 = 1; m12 = 0; m13 = 0; m14 = 0;
        m21 = 0; m22 = 1; m23 = 0; m24 = 0;
        m31 = 0; m32 = 0; m33 = 1; m34 = 0;
        m41 = x; m42 = y; m43 = z; m44 = 1;
        return this;
    }
    
    public inline function translate( x : Float, y : Float, z : Float ) : Matrix4 {
        m41 += x * m11 + y * m21 + z * m31;
        m42 += x * m12 + y * m22 + z * m32;
        m43 += x * m13 + y * m23 + z * m33;
        m44 += x * m14 + y * m24 + z * m34;
		return this;
    }
    
    public inline function cTranslate( A : Matrix4, x : Float, y : Float, z : Float ) : Matrix4 {
        m11 = A.m11; m12 = A.m12; m13 = A.m13; m14 = A.m14;
        m21 = A.m21; m22 = A.m22; m23 = A.m23; m24 = A.m24;
        m31 = A.m31; m32 = A.m32; m33 = A.m33; m34 = A.m34;
        
        m41 = A.m41 + x * A.m11 + y * A.m21 + z * A.m31;
        m42 = A.m42 + x * A.m12 + y * A.m22 + z * A.m32;
        m43 = A.m43 + x * A.m13 + y * A.m23 + z * A.m33;
        m44 = A.m44 + x * A.m14 + y * A.m24 + z * A.m34;
        return this;
    }
    
    public inline function cScaleMat( x : Float, y : Float, z : Float ) : Matrix4 {
        m11 = x; m12 = 0; m13 = 0; m14 = 0;
        m21 = 0; m22 = y; m23 = 0; m24 = 0;
        m31 = 0; m32 = 0; m33 = z; m34 = 0;
        m41 = 0; m42 = 0; m43 = 0; m44 = 1;
        return this;
    }
    
    public inline function scale( x : Float, y : Float, z : Float ) : Matrix4 {
        m11 *= x; m12 *= x; m13 *= x; m14 *= x;
        m21 *= y; m22 *= y; m23 *= y; m24 *= y;
        m31 *= z; m32 *= z; m33 *= z; m34 *= z;
        return this;
    }
    
    public inline function cScale( A : Matrix4, x : Float, y : Float, z : Float ) : Matrix4 {
        m11 = A.m11 * x; m12 = A.m12 * x; m13 = A.m13 * x; m14 = A.m14 * x;
        m21 = A.m21 * y; m22 = A.m22 * y; m23 = A.m23 * y; m24 = A.m24 * y;
        m31 = A.m31 * z; m32 = A.m32 * z; m33 = A.m33 * z; m34 = A.m34 * z;
        m41 = A.m41; m42 = A.m42; m43 = A.m43; m44 = A.m44;
        return this;
    }
    
    public inline function cRotationMat( radians : Float, x : Float, y : Float, z : Float ) : Matrix4 {
		// TODO: Assert x y z is a normalized vector
		var c = Math.cos( radians );
		var s = Math.sin( radians );
		var ci = 1-c;
		
		var sx = s*x;
		var sy = s*y;
		var sz = s*z;
		
		var cxy = x*y*ci;
		var cyz = y*z*ci;
		var cxz = x*z*ci;
		
		m11 = x*x*ci+c;
		m21 = cxy-sz;
		m31 = cxz + sy;
		m12 = cxy+sz;
		m22 = y*y*ci+c;
		m32 = cyz-sx;
		m13 = cxz-sy;
		m23 = cyz+sx;
		m33 = z*z*ci+c;
		m41 = 0;
		m42 = 0;
		m43 = 0;
        m44 = 1;
        m14 = m24 = m34 = 0;
        return this;
    }
    
    public inline function rotate( radians : Float, x : Float, y : Float, z : Float ) : Matrix4 {
		// TODO: Assert x y z is a normalized vector
		var c = Math.cos( radians );
		var s = Math.sin( radians );
		var ci = 1-c;
		
		var sx = s*x;
		var sy = s*y;
		var sz = s*z;
		
		var cxy = x*y*ci;
		var cyz = y*z*ci;
		var cxz = x*z*ci;
		
		var a11 = x*x*ci+c;
		var a21 = cxy-sz;
		var a31 = cxz + sy;
		var a12 = cxy+sz;
		var a22 = y*y*ci+c;
		var a32 = cyz-sx;
		var a13 = cxz-sy;
		var a23 = cyz+sx;
		var a33 = z*z*ci+c;
		    
        var t11 = a11 * m11 + a12 * m21 + a13 * m31;
        var t12 = a11 * m12 + a12 * m22 + a13 * m32;
        var t13 = a11 * m13 + a12 * m23 + a13 * m33;
        var t14 = a11 * m14 + a12 * m24 + a13 * m34;
        var t21 = a21 * m11 + a22 * m21 + a23 * m31;
        var t22 = a21 * m12 + a22 * m22 + a23 * m32;
        var t23 = a21 * m13 + a22 * m23 + a23 * m33;
        var t24 = a21 * m14 + a22 * m24 + a23 * m34;
        m31 = a31 * m11 + a32 * m21 + a33 * m31;
        m32 = a31 * m12 + a32 * m22 + a33 * m32;
        m33 = a31 * m13 + a32 * m23 + a33 * m33;
        m34 = a31 * m14 + a32 * m24 + a33 * m34;
        
        m11 = t11; m12 = t12; m13 = t13; m14 = t14;
        m21 = t11; m22 = t12; m23 = t13; m24 = t24;
        
        return this;
    }
    
    public inline function cRotate( A : Matrix4,  radians : Float, x : Float, y : Float, z : Float ) : Matrix4 {
        // TODO: Assert x y z is a normalized vector
		var c = Math.cos( radians );
		var s = Math.sin( radians );
		var ci = 1-c;
		
		var sx = s*x;
		var sy = s*y;
		var sz = s*z;
		
		var cxy = x*y*ci;
		var cyz = y*z*ci;
		var cxz = x*z*ci;
		
		var a11 = x*x*ci+c;
		var a21 = cxy-sz;
		var a31 = cxz + sy;
		var a12 = cxy+sz;
		var a22 = y*y*ci+c;
		var a32 = cyz-sx;
		var a13 = cxz-sy;
		var a23 = cyz+sx;
		var a33 = z*z*ci+c;
		    
        m11 = a11 * A.m11 + a12 * A.m21 + a13 * A.m31;
        m12 = a11 * A.m12 + a12 * A.m22 + a13 * A.m32;
        m13 = a11 * A.m13 + a12 * A.m23 + a13 * A.m33;
        m14 = a11 * A.m14 + a12 * A.m24 + a13 * A.m34;
        m21 = a21 * A.m11 + a22 * A.m21 + a23 * A.m31;
        m22 = a21 * A.m12 + a22 * A.m22 + a23 * A.m32;
        m23 = a21 * A.m13 + a22 * A.m23 + a23 * A.m33;
        m24 = a21 * A.m14 + a22 * A.m24 + a23 * A.m34;
        m31 = a31 * A.m11 + a32 * A.m21 + a33 * A.m31;
        m32 = a31 * A.m12 + a32 * A.m22 + a33 * A.m32;
        m33 = a31 * A.m13 + a32 * A.m23 + a33 * A.m33;
        m34 = a31 * A.m14 + a32 * A.m24 + a33 * A.m34;
        m41 = A.m41;
        m42 = A.m42;
        m43 = A.m43;
        m44 = A.m44;
        
        return this;
    }
    
    public inline function mul( A : Matrix4 ) : Matrix4 {
        var t11 = A.m11 * m11 + A.m12 * m21 + A.m13 * m31 + A.m14 * m41;
        var t12 = A.m11 * m12 + A.m12 * m22 + A.m13 * m32 + A.m14 * m42;
        var t13 = A.m11 * m13 + A.m12 * m23 + A.m13 * m33 + A.m14 * m43;
        var t14 = A.m11 * m14 + A.m12 * m24 + A.m13 * m34 + A.m14 * m44;
        var t21 = A.m21 * m11 + A.m22 * m21 + A.m23 * m31 + A.m24 * m41;
        var t22 = A.m21 * m12 + A.m22 * m22 + A.m23 * m32 + A.m24 * m42;
        var t23 = A.m21 * m13 + A.m22 * m23 + A.m23 * m33 + A.m24 * m43;
        var t24 = A.m21 * m14 + A.m22 * m24 + A.m23 * m34 + A.m24 * m44;
        var t31 = A.m31 * m11 + A.m32 * m21 + A.m33 * m31 + A.m34 * m41;
        var t32 = A.m31 * m12 + A.m32 * m22 + A.m33 * m32 + A.m34 * m42;
        var t33 = A.m31 * m13 + A.m32 * m23 + A.m33 * m33 + A.m34 * m43;
        var t34 = A.m31 * m14 + A.m32 * m24 + A.m33 * m34 + A.m34 * m44;
        m41 = A.m41 * m11 + A.m42 * m21 + A.m43 * m31 + A.m44 * m41;
        m42 = A.m41 * m12 + A.m42 * m22 + A.m43 * m32 + A.m44 * m42;
        m43 = A.m41 * m13 + A.m42 * m23 + A.m43 * m33 + A.m44 * m43;
        m44 = A.m41 * m14 + A.m42 * m24 + A.m43 * m34 + A.m44 * m44;
        
        m11 = t11; m12 = t12; m13 = t13; m14 = t14;
        m21 = t11; m22 = t12; m23 = t13; m24 = t24;
        m31 = t11; m32 = t12; m33 = t13; m34 = t34;
        return this;
    }
    
    public inline function cMul( A : Matrix4, B : Matrix4 ) : Matrix4 {
        m11 = B.m11 * A.m11 + B.m12 * A.m21 + B.m13 * A.m31 + B.m14 * A.m41;
        m12 = B.m11 * A.m12 + B.m12 * A.m22 + B.m13 * A.m32 + B.m14 * A.m42;
        m13 = B.m11 * A.m13 + B.m12 * A.m23 + B.m13 * A.m33 + B.m14 * A.m43;
        m14 = B.m11 * A.m14 + B.m12 * A.m24 + B.m13 * A.m34 + B.m14 * A.m44;
        m21 = B.m21 * A.m11 + B.m22 * A.m21 + B.m23 * A.m31 + B.m24 * A.m41;
        m22 = B.m21 * A.m12 + B.m22 * A.m22 + B.m23 * A.m32 + B.m24 * A.m42;
        m23 = B.m21 * A.m13 + B.m22 * A.m23 + B.m23 * A.m33 + B.m24 * A.m43;
        m24 = B.m21 * A.m14 + B.m22 * A.m24 + B.m23 * A.m34 + B.m24 * A.m44;
        m31 = B.m31 * A.m11 + B.m32 * A.m21 + B.m33 * A.m31 + B.m34 * A.m41;
        m32 = B.m31 * A.m12 + B.m32 * A.m22 + B.m33 * A.m32 + B.m34 * A.m42;
        m33 = B.m31 * A.m13 + B.m32 * A.m23 + B.m33 * A.m33 + B.m34 * A.m43;
        m34 = B.m31 * A.m14 + B.m32 * A.m24 + B.m33 * A.m34 + B.m34 * A.m44;
        m41 = B.m41 * A.m11 + B.m42 * A.m21 + B.m43 * A.m31 + B.m44 * A.m41;
        m42 = B.m41 * A.m12 + B.m42 * A.m22 + B.m43 * A.m32 + B.m44 * A.m42;
        m43 = B.m41 * A.m13 + B.m42 * A.m23 + B.m43 * A.m33 + B.m44 * A.m43;
        m44 = B.m41 * A.m14 + B.m42 * A.m24 + B.m43 * A.m34 + B.m44 * A.m44;
		return this;
    }

}